/*
 * filter.c
 *
 *  Created on: Dec 14, 2024
 *      Author: zxx
 */
#include "filter.h"

Control_Driver_SecondOrderIirfpLoop_t bus_filter_out = {0};
Control_Driver_FirstOrderFilter_t bus_filter_out1 = {0};

void Filter_Init()
{
	bus_filter_out.s32A1 = ADC_SERVICE_BUS_VOLTAGE_BAND_GAP_50HZ_A1;
	bus_filter_out.s32A2 = ADC_SERVICE_BUS_VOLTAGE_BAND_GAP_50HZ_A2;
	bus_filter_out.s32B0 = ADC_SERVICE_BUS_VOLTAGE_BAND_GAP_50HZ_B0;
	bus_filter_out.s32B1 = ADC_SERVICE_BUS_VOLTAGE_BAND_GAP_50HZ_B1;
	bus_filter_out.s32B2 = ADC_SERVICE_BUS_VOLTAGE_BAND_GAP_50HZ_B2;
	bus_filter_out1.s16Ka = 16384U;//0.7
}




int16_t Control_Driver_UpDownLimit(int16_t var, int16_t UpLimit, int16_t DownLimit)
{

	if(var > UpLimit)
	{
		return UpLimit;
	}
	else if(var < DownLimit)
	{
		return DownLimit;
	}
	else
	{
		return var;
	}
}



/*****************************************************************
功能: 二阶无限脉冲响应滤波器递归实现(IIR)
*              1 + B1*X(n-1) + B2*X(n-2)
*  Y(n) = B0 ----------------------------
*              1 + A1*Y(n-1) + A2*Y(n-1)
*******************************************************************/
int16_t Control_Driver_SecondOrderIirfp(Control_Driver_SecondOrderIirfpLoop_t *tSecondOrderIirfpLoop, uint16_t u16MathQ)
{
    int64_t s64Temp;
    int64_t s64Output;

#if(Limit_EN)
    s64Temp = (int64_t)(Control_Driver_UpDownLimit(tSecondOrderIirfpLoop->s16Input, tSecondOrderIirfpLoop->s16InputUpperLimit, tSecondOrderIirfpLoop->s16InputLowerLimit));
#else
    s64Temp = tSecondOrderIirfpLoop->s16Input;
#endif
    s64Temp -= (int64_t)(((int64_t)tSecondOrderIirfpLoop->s32A1 * tSecondOrderIirfpLoop->s64X1) >> u16MathQ);
    s64Temp -= (int64_t)(((int64_t)tSecondOrderIirfpLoop->s32A2 * tSecondOrderIirfpLoop->s64X2) >> u16MathQ);

    s64Output = s64Temp;

    s64Output += (int64_t)(((int64_t)tSecondOrderIirfpLoop->s32B1 * tSecondOrderIirfpLoop->s64X1) >> u16MathQ);
    s64Output += (int64_t)(((int64_t)tSecondOrderIirfpLoop->s32B2 * tSecondOrderIirfpLoop->s64X2) >> u16MathQ);

    tSecondOrderIirfpLoop->s64X2 = tSecondOrderIirfpLoop->s64X1;
    tSecondOrderIirfpLoop->s64X1 = s64Temp;


    tSecondOrderIirfpLoop->s16Output = (int16_t)(((int64_t)tSecondOrderIirfpLoop->s32B0 * s64Output) >> u16MathQ);
#if(Limit_EN)
    tSecondOrderIirfpLoop->s16Output = Control_Driver_UpDownLimit(tSecondOrderIirfpLoop->s16Output, tSecondOrderIirfpLoop->s16OutputUpperLimit, tSecondOrderIirfpLoop->s16OutputLowerLimit);
#endif
    return tSecondOrderIirfpLoop->s16Output;
}


/*****************************************************************
功能: 一阶全通滤波器      Y(n)=X(n-1)+ a(Y(n-1)-X(n))
*            A1 + z^-1
* G(z) = -------------------
*          1 + A1*z^-1
*******************************************************************/
int16_t Control_Driver_FirstOrderApf(Control_Driver_FirstOrderFilter_t *tFirstOrderApf, uint16_t u16MathQ)
{
    int32_t s32Temp;

    tFirstOrderApf->s16OutputLast = tFirstOrderApf->s16Output;


    s32Temp = (int64_t)(((int64_t)tFirstOrderApf->s16Ka * (tFirstOrderApf->s16OutputLast - tFirstOrderApf->s16Input))>>u16MathQ);
#if(Limit_EN)
    s32Temp = Control_Driver_UpDownLimit(s32Temp, (int32_t)tFirstOrderApf->s16OutputUpperLimit, (int32_t)tFirstOrderApf->s16OutputLowerLimit);
#endif
    tFirstOrderApf->s16Output = tFirstOrderApf->s16InputLast + (int16_t)s32Temp;
    tFirstOrderApf->s16InputLast = tFirstOrderApf->s16Input;

    return tFirstOrderApf->s16Output;
}

